自碰撞检测算法使用文档

[TOC]

1 接口介绍

在机械臂末端安装基础几何体来进行自碰撞检测， 目前算法支持如下基础几何体类型：球体、圆柱体、 圆锥体、 长方体、 平面、 三角形网格（mesh）、点云， 通过下面接口将其添加到机器人的碰撞几何体中（所添加的几何体与机器人连杆固连成为机器人的一部分，可以进行自碰撞检测）

    /**
     * @brief 设置机械臂连杆对应的碰撞几何体模型
     * @param link_name: 连杆名称，通过　mdlGetLinkName　获得
     * @param shapes: 几何模型，支持包括包络球在内的多种几何体类型
     * @param origins: 几何模型参考坐标系在连杆坐标系的位姿
     * @return　< 0, 表示设置失败
     */
    ARAL_API_COMMON(1.0) int mdlSetLinkCollisionGeometryModel(const std::string& link_name, const std::vector<geometric_shapes::ShapePtr>& shapes, const std::vector<RLPose>& origins)const = 0;

检测机械臂是否发生自碰撞

    /**
     * @brief 检查机械臂是否发生碰撞
     * @param joint: 机械臂的构型（关节角）
     * @param checkSelf: true:只检查自碰撞; false:则还需检测和环境的碰撞
     * @param result: 碰撞结果
     * @return: 返回值 < 0 表示计算失败
     */
    ARAL_API_COMMON(1.0) int mdlCheckRobotCollision(const RLJntArray& joint, const bool& checkSelf, GeometryCollisionResult& result)const = 0;

2 使用例程

Setup("aubo_i5");
interface::RLJntArray joint = {-0.0143431, 0.0491329, -0.0657296, -0.0633004, -0.0799256,0.230762};
interface::GeometryCollisionResult result;

geometric_shapes::Sphere* shape     = new geometric_shapes::Sphere(0.05); // 给定包络球的半径大小
shape->setShapeName("a1");
shape->setShapeType(geometric_shapes::ShapeType::SPHERE);
interface::RLPose origin            = {0, 0, 0.1, 0, 0, 0}; // 给定包络球中心基于末端法兰坐标系的位姿
std::string link_name               = "end_effector";

//! 设置机械臂连杆对应的碰撞几何体模型
int ret = robot->mdlSetLinkCollisionGeometryModel(link_name, {static_cast<geometric_shapes::ShapePtr>(shape)}, {origin});
CHECK(ret >= 0);

//! 检测机械臂包络球是否发生自碰撞
robot->mdlCheckRobotCollision(joint, true, result);
CHECK(result.collision == false);